{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 191,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import math\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline\n",
    "np.set_printoptions(threshold=np.nan)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2387,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "SPEED = 1.4\n",
    "LENGTH = 1.5\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2494,
   "metadata": {},
   "outputs": [],
   "source": [
    "class link:\n",
    "    def __init__(self, inp_state, inp_index):\n",
    "        self.state = inp_state\n",
    "        self.index = inp_index\n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def rad_to_deg(delta):\n",
    "    return delta * 180 / math.pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def deg_to_rad(delta):\n",
    "    return delta * math.pi/180"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 916,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class state:\n",
    "\n",
    "    def __init__(self, x, y, theta, g,f, steer):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.theta = theta\n",
    "        self.g = g\n",
    "        self.f = f\n",
    "        self.steer = steer\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2390,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def normalize(theta):\n",
    "    if theta < 0:    \n",
    "        theta = theta + 2.0 * math.pi\n",
    "        return theta\n",
    "    if theta > 2*np.pi:    \n",
    "        theta = theta - 2.0 * math.pi\n",
    "        return theta\n",
    "    else:\n",
    "        return theta "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2227,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def heuristic(x,y,goal):\n",
    "    distance  = math.sqrt( ((x-goal.x)**2) + ((y-goal.y)**2) )\n",
    "    return distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 739,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def expand(v_state, goal):\n",
    "    next_states = []\n",
    "    for delta in range(-35, 40, 5): \n",
    "        # Create a trajectory with delta as the steering angle using the bicycle model:\n",
    "\n",
    "        # ---Begin bicycle model---\n",
    "        delta_rad = deg_to_rad(delta)\n",
    "        omega = SPEED/LENGTH * delta_rad\n",
    "        \n",
    "        d_x = SPEED * math.cos(v_state.theta)\n",
    "        d_y = SPEED * math.sin(v_state.theta)\n",
    "        next_x = v_state.x + d_x\n",
    "        next_y = v_state.y + d_y\n",
    "        next_theta = normalize(v_state.theta + (omega))\n",
    "        \n",
    "        # ---End bicycle model-----\n",
    "        next_g = v_state.g + np.sqrt( d_x * d_x +  d_y * d_y )\n",
    "        next_f = next_g + heuristic(next_x, next_y, goal)\n",
    "\n",
    "        # Create a new State object with all of the \"next\" values.\n",
    "        new_state = state(next_x, next_y, next_theta, next_g, next_f)\n",
    "        next_states.append(new_state)\n",
    "\n",
    "    return next_states"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2391,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def transition(v_state, delta, dt, goal):\n",
    "   \n",
    "    # ---Begin bicycle model---\n",
    "    #delta_rad = deg_to_rad(delta)\n",
    "    omega = SPEED/LENGTH * math.tan(delta)\n",
    "    \n",
    "    \n",
    "    d_x = SPEED * math.cos(v_state.theta) * dt\n",
    "    d_y = SPEED * math.sin(v_state.theta) * dt\n",
    "    \n",
    "    next_x = v_state.x + d_x\n",
    "    next_y = v_state.y + d_y\n",
    "    next_theta = normalize(v_state.theta + (omega*dt)) \n",
    "\n",
    "    # ---End bicycle model-----\n",
    "    next_g = v_state.g + np.sqrt( d_x * d_x +  d_y * d_y )\n",
    "    next_f = next_g+heuristic(next_x, next_y, goal)\n",
    "    next_steer = delta\n",
    "\n",
    "    # Create a new State object with all of the \"next\" values.\n",
    "    new_state = state(next_x, next_y, next_theta, next_g, next_f, next_steer)\n",
    "\n",
    "    return new_state"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1899,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "w = np.array([[1, 0],[-1,0]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1900,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "vehicle_points = np.array([[-1.1, 2],[0, 2],[1.1, 2],[-1.1, 0],[0, 0],[1.1,0],[-1.1, -2],[0, -2],[1.1, -2]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def rotation(w,t):\n",
    "    y = np.array([[math.cos(t), math.sin(t)], [-math.sin(t), math.cos(t)]])\n",
    "    return np.matmul(w, y)\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def transformation(w, x, y,t):\n",
    "    p = np.zeros_like(w)\n",
    "    for i in range(p.shape[0]):\n",
    "        p[i][0:2] = w[i][0:2]+[x,y]\n",
    "        \n",
    "    return rotation(p,(t-np.pi/2))\n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1359,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[-0.1,  2. ],\n",
       "       [ 1. ,  2. ],\n",
       "       [ 2.1,  2. ],\n",
       "       [-0.1,  0. ],\n",
       "       [ 1. ,  0. ],\n",
       "       [ 2.1,  0. ],\n",
       "       [-0.1, -2. ],\n",
       "       [ 1. , -2. ],\n",
       "       [ 2.1, -2. ]])"
      ]
     },
     "execution_count": 1359,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "transformation(vehicle_points, 1.0, 0.0, np.pi/2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[  2.00000000e+00   1.10000000e+00]\n",
      " [  2.00000000e+00   1.22464680e-16]\n",
      " [  2.00000000e+00  -1.10000000e+00]\n",
      " [ -6.73555740e-17   1.10000000e+00]\n",
      " [  0.00000000e+00   0.00000000e+00]\n",
      " [  6.73555740e-17  -1.10000000e+00]\n",
      " [ -2.00000000e+00   1.10000000e+00]\n",
      " [ -2.00000000e+00  -1.22464680e-16]\n",
      " [ -2.00000000e+00  -1.10000000e+00]]\n"
     ]
    }
   ],
   "source": [
    "print(rotation(vehicle_points, deg_to_rad(-90)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 793,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "grid_x_m = 20\n",
    "grid_y_m = 20\n",
    "\n",
    "coll_cell_side = 1\n",
    "grid_coll_x = np.int( np.ceil(grid_x_m/coll_cell_side) )\n",
    "grid_coll_y = np.int( np.ceil(grid_y_m/coll_cell_side) )\n",
    "\n",
    "grid = np.zeros((grid_coll_x,grid_coll_y), np.int)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 800,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 801,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "grid[9][0:18] = 1\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 802,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]]\n"
     ]
    }
   ],
   "source": [
    "print(grid)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1361,
   "metadata": {},
   "outputs": [],
   "source": [
    "cell_side = 1.0\n",
    "x_offset = (grid.shape[1]*coll_cell_side)/2\n",
    "y_offset = 0.0\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1362,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_grid_coordinates(x, y):\n",
    "    x = x+x_offset\n",
    "    y = y+y_offset\n",
    "    cell_x = math.floor(x/cell_side)\n",
    "    cell_y = math.floor(y/cell_side)\n",
    "    #grid[cell_y, cell_x] = 2\n",
    "    return cell_x, cell_y\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1363,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def check_occupancy(x, y):\n",
    "    x = x+x_offset\n",
    "    y = y+y_offset\n",
    "    cell_x = math.floor(x/coll_cell_side)\n",
    "    cell_y = math.floor(y/coll_cell_side)\n",
    "    #grid[cell_y, cell_x] = 2\n",
    "    return grid[cell_y, cell_x]\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2151,
   "metadata": {},
   "outputs": [],
   "source": [
    "def delta_t(delta, n, off):\n",
    "    #delta = deg_to_rad(delta)#for convenient debugging\n",
    "    #off = deg_to_rad(off)#for convenient debugging\n",
    "    delta_list = []\n",
    "    delta_list.append(delta)\n",
    "    delta_calc = delta\n",
    "    for i in range(0 ,n):\n",
    "        delta_calc += off   \n",
    "        if delta_calc < max_angle:\n",
    "            delta_list.append(delta_calc)\n",
    "        \n",
    "    delta_calc = delta\n",
    "    for i in range(0 ,n):\n",
    "        delta_calc -= off\n",
    "        if min_angle < delta_calc:\n",
    "            delta_list.append(delta_calc)\n",
    "        \n",
    "    return delta_list"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2310,
   "metadata": {},
   "outputs": [],
   "source": [
    "def checkLimits( x,y ):\n",
    "    \n",
    "    result = True\n",
    "    \n",
    "    if  ( x < -grid_x_m/2 ) | \\\n",
    "        ( y < 0 ) | \\\n",
    "        ( x > grid_x_m/2 ) | \\\n",
    "        ( y > grid_y_m ):\n",
    "        \n",
    "        result = False\n",
    "    \n",
    "    return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2317,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "max_angle = 0.785398\n",
    "min_angle = -0.785398"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2478,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def check_closed(x, y, theta):\n",
    "    x = x + x_state_offset\n",
    "    y = y + y_state_offset\n",
    "    \n",
    "    cell_x = math.floor(x/state_cell_side)\n",
    "    cell_y = math.floor(y/state_cell_side)\n",
    "    \n",
    "    cell_theta = math.floor( theta * NUM_THETA_CELLS / (2.0 * np.pi) )\n",
    "    return closed[cell_theta][cell_y][cell_x]\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2479,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def set_closed(x, y, theta):\n",
    "    x = x + x_state_offset\n",
    "    y = y + y_state_offset\n",
    "    \n",
    "    cell_x = math.floor(x/state_cell_side)\n",
    "    cell_y = math.floor(y/state_cell_side)\n",
    "    \n",
    "    cell_theta = math.floor( theta * NUM_THETA_CELLS / (2.0 * np.pi) )\n",
    "    closed[cell_theta][cell_y][cell_x] = 1\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2495,
   "metadata": {},
   "outputs": [],
   "source": [
    "state_cell_side = 1\n",
    "grid_state_x = np.int( np.ceil(grid_x_m/state_cell_side) )\n",
    "grid_state_y = np.int( np.ceil(grid_y_m/state_cell_side) )\n",
    "\n",
    "NUM_THETA_CELLS = 360\n",
    "closed = [[[0 for x in range(grid_state_x)] for y in range(grid_state_y)] for cell in range(NUM_THETA_CELLS)]\n",
    "\n",
    "x_state_offset = grid_x_m/2.0\n",
    "y_state_offset = 0.0\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2496,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "current_state = state(0.0, 2.1, np.pi/2, 0.0, 0.0, 0.0)\n",
    "new_points = np.copy(vehicle_points)\n",
    "#print(new_points)\n",
    "\n",
    "Goal.x = 0.0\n",
    "Goal.y = 18.0\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2497,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#print(current_state.x, current_state.y, current_state.theta)\n",
    "\n",
    "path_state = current_state\n",
    "open_list = []\n",
    "path_link = link( path_state, 0 )\n",
    "open_list.append( path_link )\n",
    "\n",
    "trace_list = []\n",
    "trace_link = link( path_state, -1 )\n",
    "trace_list.append( trace_link )\n",
    "\n",
    "\n",
    "path_x = []\n",
    "path_y = []\n",
    "\n",
    "path_x.append( current_state.x )\n",
    "path_y.append( current_state.y )\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2498,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "goal\n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAXQAAAD8CAYAAABn919SAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\nAAALEgAACxIB0t1+/AAAADl0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uIDIuMS4wLCBo\ndHRwOi8vbWF0cGxvdGxpYi5vcmcvpW3flQAAIABJREFUeJzt3X90lNeZH/Dvo0HIgqAFjgWJZYi8\nhMUbIoK2aoDqtCVOibUocSbZuoRC67PxQs82m67DLltR2GAlYq0NscOedtses3Gdc6wQx6mtdYpj\nosbr+pQDSuUKIxNDMbEiI7xGOVhrL5b5IZ7+MTPyaDQz76/7/pzv5xwO0sw7817Nj2fu3Pvc54qq\ngoiI4q8q7AYQEZEZDOhERAnBgE5ElBAM6ERECcGATkSUEAzoREQJwYBORJQQDOhERAnBgE5ElBCz\ngjzZzTffrI2NjUGekogo9l588cVfqWq91XGBBvTGxkYMDAwEeUoiotgTkV/aOY5DLkRECcGATkSU\nEAzoREQJYRnQReQREbkoIi/nXbZaRI6LyAkRGRCRT/jbTCIismKnh/4ogLaCy74JoFNVVwP4WvZ3\nIiIKkWWWi6q+ICKNhRcDqMv+/GsALphtFlF4egdHsf/IGVwYn8At82ux884VSDc3hN0sIktu0xbv\nA3BERL6FTC//H5lrElF4egdHsevJIUxcmwQAjI5PYNeTQwDAoE6R53ZS9PcBfFVVlwD4KoDvlDpQ\nRLZnx9kHxsbGXJ6OKBj7j5yZCuY5E9cm0fmjUyG1iMg+twH9HgBPZn9+AkDJSVFVfVhVW1S1pb7e\ncqETUagujE8Uvfytd69hw0PPB9sYIofcBvQLAP5p9uc7AJw10xyicN0yv7bkdWcvXsaqvc8G2Boi\nZ+ykLR4CcAzAChE5LyL3AtgG4EEReQnAnwHY7m8ziYKx884VZa9/+8okthw8FlBriJyxk+WyucRV\n/8BwW4hCl25uwP1Pn8L4xLWSxxw9dynAFhHZx5WiRAXuv2ul5TGNHYcDaAmRMwzoRAXspic2dhzm\nmDpFCgM6kQdvX5lkb50igwGdqIjWZQsdHd/YcZiTpRQ6BnSiInq2rXMc1I+eu8TeOoVKVDWwk7W0\ntCh3LKK4cROk59dW4/67VrJcABkhIi+qaovVceyhE1kY7m5HXU3K0W3GJ67hvsdPcCiGAsWATmTD\nyc42DHe3u7oth2IoKAzoRA64DeoAc9fJfwzoRA4Nd7fjppS4uu3tu58x3Bqi9zGgE7lwet9GV731\n9yaVQZ18w4BO5MFwdzu2rl3q6DbvTSqHX8gXDOhEHnWlm1wFdpYNINOYh07kAyc98AObVjNfncpi\nHjpRiJxMnN73+An0Do763CKqBAzoRD45vW+j7aC+/8gZn1tDlcDOjkWPiMhFEXm54PKviMgZETkl\nIt/0r4lE8XV630Zbx42W2MuUyAk7PfRHAbTlXyAinwTwOQCrVHUlgG+ZbxpRMtgtHcB0RvLKzhZ0\nL4hIY8HFvw+gW1WvZI+5aL5pRPG3Zl8f3nznqq1j35sMLkGBksntGPpvAPjHItIvIv9LRP5hqQNF\nZLuIDIjIwNjYmMvTEcWPk2Cew1RG8sJtQJ8FYAGAtQB2AviBiBSd/VHVh1W1RVVb6uvrXZ6OKH6c\nBnMgswMSqzOSW24D+nkAT2rGzwDcAHCzuWYRVa6j5y6F3QSKKbcBvRfAHQAgIr8BYDaAX5lqFFGl\nY2kAcsNO2uIhAMcArBCR8yJyL4BHAPx6NpXx+wDu0SCXnBLFwOJ5sz3dnkGdnOLSfyIfuZkYLeSl\nBjslA5f+E0VA/+4NGO5ux4FNq9Ewv9bVfTR2HGZpALLFMg+diLxLNzdMFeDacvCY44nPHY+fmLof\nolLYQycKWM+2dY7L7d5ApogXUTkcQyeKAKcToHU1KZzsbLM+kBKBY+hECfb2lUk0dhzmIiSahmPo\nRDF29NwlrNr7rGVvvXdwFPc/fQrjE9cAAAvmVGPvZ1dyTD5hGNCJIqB12ULXK0RzvXUgk/vev3vD\ntOt7B0ex4wcncCNvdPWtd69h5w9fAsCJ1iThGDpRRLjJfimnYX4tPnl7PXr6R1Dqbd4wvxZHO+4w\ndk7yB8fQiWImP/slVbzWnSOj4xN47HjpYJ47hpKDAZ0oYrrSTTj3wEYMd7ejddlC38/HRUvJwYBO\nFGG5Xruf/viJl3y9fwoOAzpRDPjZU79+Q7Gnd8i3+6fgMKATxUDPtnW+BvXv9Y/MuKx3cBSt3c/h\nto7DaO1+jkMzMcC0RaKY6Nm2bsZlGx56HmcvXvZ83zc0E8BzKYy9g6PY9eQQJq5NAshMnu56MtOL\nZ5pjdDFtkSgh9vQO4VD/65h0+Z4WAK9lx+tbu58rmgEzd3YKp77OkgNBM5a2KCKPiMjF7GYWhdf9\nsYioiHD7OaKQ5WfHDHe3o64m5ej2ivczXkqlM16+yj1Po8zOGPqjAGZ8JIvIEgAbAMwcfCOi0J3s\nbHM87m6nouPRc5c4nh5RlgFdVV8AUGz52rcB/AkyH+xEFEFuSvXaqfy4/8gZL80in7jKchGRuwCM\nqioTWIlioCvdNDUUYwJXmEaT44AuInMA7AbwNZvHbxeRAREZGBsbc3o6IjLMVPojc9ejx00PfRmA\n2wC8JCLDAG4F8H9F5IPFDlbVh1W1RVVb6uvr3beUiIzo2bYO3ivFAI8d5/RZ1DgO6Ko6pKqLVLVR\nVRsBnAfwW6r6t8ZbR0S+2OJgTL2cDQ89b+R+yAw7aYuHABwDsEJEzovIvf43i4j8sqd3yFjv+uzF\ny8x4iRDLlaKqutni+kZjrSEi3x3qf93o/XX+6BRXj0YEa7kQVRi3K0lLeevda5wgjQgGdKIKY2Lz\njEKPHR/h0EsEMKATVZjNa5b4cr+7n2IvPWwM6EQVpivdZGybu3yXr07iI7usV5mSf1htkYgAeK/W\nmM/vXZYqjd1qiwzoRDTDnt4h9Bwf8VSoiUHdHGPlc4mo8nSlm3DL/FpP99HYcZildgPGgE5ERV0w\nUIDr6LlLHFcPEAM6ERXltYeec13BoB4QBnQiKmrnnStQnTKTCXNdOQQTBAZ0Iioq3dyA/f/841gw\np9rYfR49dwmr9j5r7P5oOma5EJErXot8tS5biJ5t6wy2KLmYtkhEgVmzrw9vvnPV1W23rl2KrnST\n4RYlCwM6EQXKa489JYLNa5YwuBfBPHQiClRu39Lli+a6uv2kKh47PsLKjR4woBORUX071k9tSF1X\nk3J8e25t556dHYseEZGLIvJy3mX7ReS0iJwUkadEZL6/zSSiODrZ2WZsU2qyZqeH/iiAtoLL+gB8\nTFVXAfh/AHYZbhcRJUTPtnWO67qwtro7lgFdVV8AcKngsp+o6vXsr8cB3OpD24goQZz01PcfOeNj\nS5LLxBj6lwD82MD9EFGC9Wxbh5tsrjwdHZ9Aa/dz7Kk75Cmgi8huANcB9JQ5ZruIDIjIwNjYmJfT\nEVHMnd63EbNsVhMYHZ/AHz3xEoO6A64DuojcA+AzALZomWR2VX1YVVtUtaW+vt7t6YgoIV59wP54\n+uQN5dZ2DrgK6CLSBuDfA7hLVd812yQiSjon6YyXr0762JJksZO2eAjAMQArROS8iNwL4D8BmAeg\nT0ROiMh/9bmdRJQgJzvbYHZHUwKAWVYHqOrmIhd/x4e2EFEFea27HR/ZdRjXg6s+knhcKUpEoXEy\nnk7WGNCJKFQcejGHAZ2IQvWaw1WkVBoDOhFF3m0d3JPUDgZ0IgqdVVkAzpvaw4BORKHjVnRmMKAT\nUSS4qZ1O0zGgE1EknOwsrNI93W0dh1mwywIDOhHFgiJTsGvnD1mwqxQGdCKKlWuTis4fnQq7GZHE\ngE5EsfPWu9fCbkIkMaATESUEAzoRRYbdberm11b73JJ4YkAnosiwm49+/10rfW5JPDGgE1GsCIB0\nc0PYzYgkBnQiiowtB49ZH7N2aQAtiSc7OxY9IiIXReTlvMsWikifiJzN/r/A32YSUSU4eu6S5TFd\n6aYAWhJPdnrojwIoXMLVAeCnqrocwE+zvxMR+aphfm3YTYg0y4Cuqi8AKPzY/ByA72Z//i6AtOF2\nERHNsPPOFWE3IdLcjqEvVtU3ACD7/6JSB4rIdhEZEJGBsbExl6cjIuJkqBXfJ0VV9WFVbVHVlvr6\ner9PR0QJxeEWa24D+psi8iEAyP5/0VyTiIhm4nCLtVkub/c0gHsAdGf//2tjLSKiimSVspg/3LLl\n4LFpGTGtyxZykwzYS1s8BOAYgBUicl5E7kUmkG8QkbMANmR/JyJyzU7KIgCs2vvsjGOPnrtkK4c9\n6Sx76Kq6ucRVnzLcFiKqUHaD8ZaDx/D2lcmi19n9QEgyrhQlotBZBeOUiK3jKh0DOhFF3qQqh1Rs\nYEAnoshbMKeavXMbGNCJKHRWddBVA2pIzLlNWyQHegdHsfupIVy+Wnwyp5yG+bXYeecKWyvk9vQO\n4Xv9I7iRffGnBJgs8kaoAnAj7/fli+aib8d6W+3Z8NDzOHvxsqvblrOndwiH+l/HpCpSIrj5A9V4\n852rU9cXpqUVS1u7u2Up9h85gwvjE7il4HErPL66Crh+AxDB1OMFAIvnzUb/7g3Y0zuEx46PTGtj\nFYCHNq12tVqxd3AU9z1+YtpluXOVUvh81lZX4YEvrMITAyMzeqt1Nalpk4VO0/gKH59ivKQGFrv/\nXBC30/Men7Deco6bXgCiAX70tbS06MDAQGDnC0vv4Cj2HzmD0fEJ4/ctAF7rbp9xebEA5ISdwFwY\nzAtvWyzIArDMF/ba9lJyAbhYAPRi69qlaPnwwqIfHsUeg4vvXCn6uAGlg7qJxyT/sS78wNy8Zgm6\n0k2ezlNXk8LJzsK6fTOt2vtsycwUUwTAt11+2MaBiLyoqi2WxzGgm1Uq6Pkh/w27bNczmPT4XFr1\nwBo7Dpe9rd2gWXgeE20vpba6ChPXblgf6FDht5za6hRuXXCTq+d+uMgHtKnHZLi7HWv29U37tmNS\nuaBe7FuJXw4kOJgD9gM6h1wMsfOV1bSj5y6VDbJu7u/23c/g9L6NM66zyjBw8rcXHutXMAfgSzAH\npgfzzHkmjX6Qm3pMTL4+inn7ymTRcwiAoLqKW9cuTXQwd4IB3YAwgrlf3pvUqTdofk/az78vJeJr\nUI+juD8mQbacG168j1kuBiQlmBfKfQPwu5e3ec0SX+8/yhbPm1308kp+TJwoNlxVyRjQKXCFKWqv\njf19SC0JX6ksl650E7Zy78yyGMxn4pBLSLauXepLZkfU1dWkZky8JvUbjhWrgNSVbsLTg6O+Z4jE\nUbG8dVZgZEA3wirDY+vapUXH+SoxoNtJczPBSdZNGOpqUpbHlCtEVckKOwW3734G7xVZcJGrwFhJ\nQZ0B3YCebetc9Q6iHnRMs1oNaMr82mr0bFtndOx/8bzZxlL/Zom9D7ZKem04USqzpphKewwZ0A1x\n0wvo2bYOt3UcDjQjoJggMiqKDbXkmP5gu/+ulcbuK8dUMK+uAs7+Wfmhlii8JiiePAV0EfkqgN9D\nJktpCMDvqup7JhqWZFFLc6yuAs7tez/I+NG+cj1Sk73p5YvmRjonef/dq8te73dGESWb6ywXEWkA\n8O8AtKjqxwCkAHzRVMOSKmrBHMCM8ceebesw3N2O4e72wIZJTGV0mKgr45fWZQvLfth8ZBeDOXnj\nNW1xFoBaEZkFYA6AC96blGxRC+Y5pXqGueBeKl/aDjvBuivd5Okcds8TplJDTnt6h9DYcRjXOc5i\nXFAdkqhwHdBVdRTAtwCMAHgDwN+p6k9MNYyCl1tE1NhxeMZS/3JVAa3YXcnn5RzFzhOlN3OpD5s1\n+/oqMtspCJWYtuhlyGUBgM8BuA3ALQDmisjWIsdtF5EBERkYGxtz39KI2XLwWNkAGHe5VaJx+rsK\n2xqVN7Og+Ifaqr3P+lY0i6Lz/AfJdbVFEbkbQJuq3pv9/V8DWKuq/7bUbZJSbbHUOHhh5bkojpeH\noVQefk6u3PCF8QnMnlWFK9fdF9QqXKwTROlWKw3za3G0446p3/m6CIbd8r5xYLfaopcx9BEAa0Vk\njogIgE8BeMXD/cVGqTfj21cmp3qJfr9pvY43B+mx4yNYs6+v6HW9g6PY+cRLGB2fgAKegjmQCeD5\nTna2QTzdY2m5jYutjI5PTPtGV6nBfPmiuVOT7cPd7b7PeeS/HyuFp3roItIJYBOA6wAGAfyeql4p\ndXxSeuhWqWXD3e2BpJ8FdZ64KTZ2avpxOrBpNQZ+eYnj3zGQhJovgdRDV9W9APZ6uY8kCirINnYc\nRlXBFmoUTCZRurkB6eYGfL9/JPLZKVZDXn7tGEXBY7XFmHMazHNfd/0ahoiKwqEXu1/vnQ4DRD2Y\nA9bVLLvSTdOGQii+uPS/wlTKEE2u3kcuQHWlm2z1Qu0clxs7L/zQiKrCna2cbDwed1FKXQ0Ce+iU\naI0dh9E7OGr0PjevWRLrSoij4xP46uMnsKd3aMZ1SZpErMQ8dPbQKfF2ZDcqtipCZjdrpSvdZPyb\nTm4uJNdGv/fkVAA9x0fQ8uHp5QjinoGzYE419n52ZUV8+yiGAT0gua+5f/k3Z8tuJlw4hlkpQyR+\nugHgvsdPWG4qktv2zSrwm3pOnA595PL1R8cnjJxfAew/csZ18Mu9Vjc89LzRDbKdqqQhJCsM6C4s\nXzTX8Qt4dHwCTwyMoG/H+pJZBfkTclx8Yt7TFkMvuUyQzWuW+Jr1sXjebFdlDnKZNTkmPlgu5H04\nuL2/cq9pP1ll71QiBnQX+nasd/XiP3ruElbtfXZq9dqh/tcxqYqUCDavWTL14lyzry/UJeHza6tR\nM0sStyzdasw7t7uN3QlUu6IceG6ZXwvA+dh54fCU6cesnEocG7eLAd2l4e52VxsR5LIvWpctxLkH\nNk67bk/vEHqOj4S2uUFh4Km0/GS/tiwzFcz9+NY2Oj6BPb1Dju+32JDUgjnVeOvda2Vvlz+k6LRT\nxKEVawzoHrzW3e76TZafSta6bCFevfj3ofWI59dW4/67Zk4kdaWb8PjPRnDN22r8KflvyN7BUez4\nwQlPi6LsBBCnojrM5WdNGlMf2ns/uxL3ZSegS8mlkpYqBVFMkmqy+I0B3SMTASDMIFJqOMBUb7C2\nugoPfGHVjA+L3Hiwlwm1wa99GkBmsnD3U0O4fDWeaYRW3KRIHti0GjsePwFDn8W2pJsbLAM64OzD\nafmiuSU3LfnIruk15GcJ8OoDlb0winnoFezAptW+BnMAeOUbv132K3LfjvU4sKn8tmxW0s0NOPX1\ntsgWLCuW7+2E0+eiYX4t0s0N+EV3O25KBbsm2M5zYDeY35SSksG82IYg15W7PjGgV6jh7vaSgTbo\nbwzp5gYMd7ejribl6X76d2+IZFB/7PiI66B+++5nHN9m550rpn4+vW8jZvkQ00utwPS6SUlOXU0K\np/dtnHF5rmplKXEoxeAnT9UWnUpKtcV8cU0vLFezw2Tuu9PaIE4mmktlO3idzLXKV3dzfEpkxiS4\nlSivQSj2vO7pHZrK3ApTEuvR2K22yIBuQByDernUL5OBxGmKWe/gqK1x2Jxyb94gFrzk5iDsPGZ2\nA42b7Cl6XyUHdE6KGmAVsKIY8Mu1p3XZQmPtdXo/6eYGY3XG+3as9/WxP7BpNdLNDbZyuO2WFYhy\nrzwO/BheihMG9AAUBvwobIsGYFraZH4bb6v/QKgfQLmJWjtB3SpvvGfbOvQOjqLzR6eMpjjmgjlg\n70MrV1agHK+Tp8QsF687Fs0H8FcAPoZMaYgvqWrJ7kpSh1zcqpTemCCTs+/U6s6fYHzCOgjb/Ypt\n9/6sVAH4hcMFMuXaWJh+R84lffVoEHuKAsBfAHhWVW8H8HFUyJ6iptj9Gh53CndlWe+/a6WR8/cO\njqK1+zkjwRwAHspLs7SThVKuJnex9DtyRmA97FkpXAd0EakD8E8AfAcAVPWqqo6balglsPM1PCcl\ngq1rl07tKhO3wv1uhnDSzQ1Yvmiup/P2Do5i15NDxioUzq+tnpbu+d5k+WhcV5MqGmys0u/IPn4e\nvs/LGPqvAxgD8N9E5OMAXgTwh6oaXh3NmMmNFRemehUW60qK/MJkdvXtWG+Z9VFuHH3/kTOYuGZu\nvsLpt4b8vzeKk+OULF4C+iwAvwXgK6raLyJ/AaADwJ/mHyQi2wFsB4ClS53t11gJutJNrgJ3HAPD\n21cmXRW/+vam1WVTGcs9FqZ65kCmt53fO7cziVlpBc7CMHe2twVpSeJlDP08gPOq2p/9/YfIBPhp\nVPVhVW1R1Zb6+noPp6Mc01uqBalU8M0NQeT+5Y+5R6W6XuG3CzuBOqnBfLi7HQc2rUZDtvxuWFJV\ngn2fT9Y3WS9cB3RV/VsAr4tIbp3xpwD83EirqKxdT54Muwme5I8d9w6OYsWeH88I9Lna8TlWmSxB\nb9jspFpg0uTmb9LNDTjaccfUvE4QdWMEmR65IFOz5sG7Px6ZD/wo8JqH/hUAPSIyG8AvAPyu9yaR\nlQlT9WxDZGdCMFc73o5SwzlW28nZlZ+RtKd3KHGbf5hwet9GY5uzJD0N0S+eArqqngBgmRtJFIRi\nwzmmtpPLz0jqSegwil1Hz13C7bufKVo8a8PKD7p+vKO8s1NcsNoiJUrh0IupAJG/PWAc0+TyB0Oq\nDIyMvDepM4advE4Af6/CPyhNYECvEK3LFqJmVvKf7revTBofT88Nt/i516ufi8wWz5uN1/LWL3jZ\nJSpf/mOx5eAxz9+EbiBTUI3cYy2XBCu1dVdUasn4xW16ZCm54RavwbxKgBtaem9Mv56XN9+56tsi\nJtP3e/biZV/2da0UDOgxY7f3WW4fxqZbfy2WeexO5P99XidGu9JNrnfCqZlVhT//nZlb8OXzs+cf\nR0fPXULv4CieGBiZ8TrlZGl5rIceE05WGeZe9IX1wJcvmos3/+69RPfOCy2eN9vTRB2Q2QrNaol/\noQVzqrH3szM33u4dHMX+I2cwOj4Bgbtl6wc2rcb9T58yVpsmbhbPm21sZ6S44AYXCeIkmOcyBYLY\n3CEu6mpSZT/EnO5QVE5h8K+ZVYUr182kmQoyq2YLV6smdfFSOZWWERNUtUUKgN1gvnje7KkXOYP5\n+6y+kZgKDMsXzZ3RkzcVzIFMb/6+x0+gseMwmr/+E/QOjqIr3eR5k+04OtT/ethNiCSOoUec3Umn\nuppUxX0NjZIDFvVmTHvr3Wu47/ETgZ4zSsLetzSqGNAjzO6O7+UmQMlfdTUp3NXcULGBNSyVspeA\nUxxyiahVe5+1PRFXLJh7rSNO1pYvmouqqqqKHMMOm5O9BCoJA3oEbTl4zHYmSqnA/eVPLjeyIpBm\nqq7KTMq9evFyxWaalONn7zm30UslTYg6wSGXiHGS0VJXk0LfjvUzLk/6wqGwVafYKy/nwX9RvAKi\nl4wcu/vGVjr20CPE6Y42hUMtGx56Ho0dhxnMffZuAqpd+mn/kTNFL+9KN02V2uWQoD/YQ48QJ8F8\n69rpuz/FYed4p1vrcQVlPNnZJSr/myW35jOHAT2mutJNnt8IuRWlVnt2euFlvLNYGqYfb/783ZGC\nUKqWS6H8VaVBOLBpNXY+cQJBfwHJLeUvlaJb2Hmh0rhSNEKC2gV++aK5M8beTQb1Yvdv2u27n3G8\nHD8MVQL8yzVmJvH8XhWav6Te9Lcju6+JPb1DU5umJ3WzdDcCW/ovIikAAwBGVfUz5Y5lQC8uyOBU\nqsfcOzhqJJc6yAyEKJc3KFXLxRS//vb8ycc9vUPoOT5i7IO+EmuwmBJkQN+BzK5FdQzozgUVzK0W\nH7V2P+fpq70AeC2kTIQo1TMJcpGXH393sZ60yfMcKKhFQ/YEUstFRG4F0A7gr7zcTyWLQjAH7E1k\nldK6bGFowRzIzCcsnjc7tPPnHNi0OtAVu7mskbqalLH7LNbrz9WLmV9b7fn+S2XAkBle0xYPAPgT\nZDYbKUpEtovIgIgMjI2NeTwdObV80VzLION2UnDr2qUY7m6PRH3q/t0bQg3qw93tofU8T3a2+Z4G\nmG5uwIm9n0brsoWe7udCQBO8lcp1QBeRzwC4qKovljtOVR9W1RZVbamvr3d7OnJh69qltiai3GSN\nRHG1Xv/uDRjubg8sK0Lw/oda2Pp2rDeW3907OFryup5t6zyd45b5ta5vS9a89NBbAdwlIsMAvg/g\nDhF5zEirKshNKX+WSQ93t/sWcKMYzPN1pZt8D+q5fTqj9jj07VjvOahbDYu4zWCqrU5h550rXN2W\n7HEd0FV1l6reqqqNAL4I4DlV3WqsZRXi9L6NxoO631+/oxbEiulKN3keHignytkafTvWe/pAszOf\n4mZ464EvNHFC1Gdc+h8Bp/dtNNqjdNKDcpr7HoXJR7v8GtuPw0KX3ISpW1bzKv27NziejM1tzrHh\noeddt4vKMxLQVfV5q5RFKi83TJCrVJcSwSwXHXcnb2K79dbzRblnWozp4Nu6bGEsvqHkuN3N6Oi5\nS5Ybkp/sbHN1/2cvXna96TaVx5WiEea092w3mLtZPj9LgFcfCH/yzw1Ti3CiPndQih+LkPIXCf3m\nn/4YEy7qBQSxojgp7Oahs5ZLQliNF3utgRLXYA5khqC8llUQxGPuoJi+HeuNl1R+852rnh/TqK7y\njTMG9IQoNl5sqmfm5+RiUJYvmuvpsdgSg3Hzck52tgVWK4jCw4AeYXU1Kdu9Kr/erHU1qUgsHPLK\nSy91+aK5se2d51swpxpvvcsdlpKMWS4RdrKzzeiybrdtSIqTnW2orXb+kk/KOO/ez64MuwnTcJML\n8xjQI+5kZ1skViImxQNfWOXo+DikKNqVbm7Agjne67GYwAlRfzCgU0l+bvYbFqcLW5Iw1JIvzF56\nrkzCcHc7g7lPGNCppM1rloTdhFDxA82c2uqqxH04RhEDekyEkWlS6W/ASv9AM+k9bqwdCAb0mOjZ\nts5TUA97cjVK7Pa8k/qBFkb5BlZZDAYDeoz0bFsHu4MAAkyNVzpJf8yJU80Wpyq95+1H+YZyHYZU\nlbDKYkAY0GPGaoHLLMkE8te627Hl4DE0dhx2lXsdt5otTtjpeScpu6WYmllm3/pvX5mcqkWf3+mY\nOzuFB+/+OKssBoQLi2ImF4yz6pgtAAAE90lEQVRyO6PnFO6Qbnqpd6VJ6nBLzp//ziojm4Ln23Lw\nGHq2rUv8YxdlDOgx1JVuKvum2XLwGIO5Ba+lAOIu3dxgPKB7qRVEZnDIJYH4xrLGPGhKIi97ii4R\nkb8RkVdE5JSI/KHJhhERkTNeeujXAfyRqv4mgLUAviwiHzXTLPJT67KFLCdAxiWhKmfcuR5DV9U3\nALyR/fkdEXkFQAOAnxtqG7nUumxh0WGXuppUooptkb+cpLu2LluYiKqccWdkDF1EGgE0A+g3cX/k\nTbFFSK3LFjKYkyMnO9vK9rpz3/SGu9sZzCPCc5aLiHwAwH8HcJ+qvl3k+u0AtgPA0qXJzu2NEr7B\nrKVEpqV+5l9OGbnXUeGOV+yRR5OngC4i1cgE8x5VfbLYMar6MICHgcyeol7OR2TS5jVL8NjxkaKX\n03QM3vHgOqCLiAD4DoBXVPUhc00iCkbhIq3CxVlEceOlh94K4F8BGBKR3AqF/6Cqz3hvFgVh8bzZ\nePOdq0UvrxRWi7QqFXcTiifXk6Kq+r9VVVR1laquzv5jMI+R/t0bZgTvxfNmJ7qOC72vVL2aupoU\nF17FFJf+VzgG78rFIafkES0yy++XlpYWHRgYCOx8RERJICIvqmqL1XGs5UJElBAM6ERECcGATkSU\nEAzoREQJwYBORJQQgWa5iMgYgF8GdkJvbgbwq7AbEQL+3ZWlEv/uOP7NH1bVequDAg3ocSIiA3bS\nhJKGf3dlqcS/O8l/M4dciIgSggGdiCghGNBLezjsBoSEf3dlqcS/O7F/M8fQiYgSgj10IqKEYEAv\nICJ3i8gpEbkhIi0F1+0SkVdF5IyI3BlWG/0mIveLyKiInMj+2xh2m/wiIm3Z5/NVEekIuz1BEZFh\nERnKPr+JrZgnIo+IyEUReTnvsoUi0iciZ7P/LwizjSYxoM/0MoAvAHgh/0IR+SiALwJYCaANwH8W\nkVTwzQvMt5Ne5z77/P0lgN8G8FEAm7PPc6X4ZPb5TWQKX9ajyLxf83UA+KmqLgfw0+zvicCAXkBV\nX1HVM0Wu+hyA76vqFVV9DcCrAD4RbOvIsE8AeFVVf6GqVwF8H5nnmRJCVV8AcKng4s8B+G725+8C\nSAfaKB8xoNvXAOD1vN/PZy9Lqj8QkZPZr6yJ+UpaoNKe03wK4Cci8qKIbA+7MQFbrKpvAED2/0Uh\nt8eYityxSET+J4APFrlqt6r+dambFbkstilC5R4DAP8FwDeQ+fu+AeBBAF8KrnWBSdRz6lCrql4Q\nkUUA+kTkdLY3SzFWkQFdVf+Zi5udB7Ak7/dbAVww06Lg2X0MROQggP/hc3PCkqjn1AlVvZD9/6KI\nPIXM8FOlBPQ3ReRDqvqGiHwIwMWwG2QKh1zsexrAF0WkRkRuA7AcwM9CbpMvsi/ynM8jM1GcRP8H\nwHIRuU1EZiMz6f10yG3ynYjMFZF5uZ8BfBrJfY6LeRrAPdmf7wFQ6lt57FRkD70cEfk8gP8IoB7A\nYRE5oap3quopEfkBgJ8DuA7gy6o6GWZbffRNEVmNzPDDMIB/E25z/KGq10XkDwAcAZAC8Iiqngq5\nWUFYDOApEQEyMeB7qvpsuE3yh4gcArAewM0ich7AXgDdAH4gIvcCGAFwd3gtNIsrRYmIEoJDLkRE\nCcGATkSUEAzoREQJwYBORJQQDOhERAnBgE5ElBAM6ERECcGATkSUEP8fzW9IL0g8NRoAAAAASUVO\nRK5CYII=\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7f948023bb00>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "goal_reached = 0\n",
    "\n",
    "for count in range(2):\n",
    "    while ( len(open_list) > 0 ) & (goal_reached == 0):\n",
    "\n",
    "        open_list.sort( key=lambda ste:ste.state.f )\n",
    "\n",
    "        path_link = open_list.pop(0)\n",
    "        path_state = path_link.state\n",
    "        path_index = path_link.index\n",
    "\n",
    "       # print( \"\" )\n",
    "        #print( \"Best path\")\n",
    "        #print( path_state.x, path_state.y, path_state.theta, path_state.f )\n",
    "        path_x.append( path_state.x )\n",
    "        path_y.append( path_state.y )\n",
    "\n",
    "\n",
    "        #exit = 0\n",
    "\n",
    "        #while exit == 0:\n",
    "\n",
    "        delta_angle = delta_t( path_state.steer, 5, deg_to_rad(5.0) )\n",
    "        delta_angle = [ 30 * np.pi / 180, 20 * np.pi / 180, 0, -20 * np.pi / 180, -30 * np.pi / 180 ]\n",
    "\n",
    "        #print( delta_angle )\n",
    "\n",
    "        state_list = []\n",
    "\n",
    "        for i in range(len(delta_angle)):\n",
    "            \n",
    "            new_state = transition( path_state, delta_angle[i], 0.1, Goal)\n",
    "            \n",
    "            #print( new_state.x, new_state.y, new_state.theta )\n",
    "            if check_closed( new_state.x, new_state.y, new_state.theta ) == 0:\n",
    "                if checkLimits( new_state.x , new_state.y ) == True:\n",
    "                    state_list.append(new_state)\n",
    "                    set_closed( new_state.x, new_state.y, new_state.theta )\n",
    "                    \n",
    "                    #print( new_state.x, new_state.y, new_state.steer )\n",
    "\n",
    "        collision_free = []\n",
    "\n",
    "        for i in range(len(state_list)):\n",
    "\n",
    "            check = 0\n",
    "            #new_points = transformation(vehicle_points, state_list[i].x, state_list[i].y, state_list[i].theta)\n",
    "            new_points = [ [ state_list[i].x, state_list[i].y ] ]\n",
    "            \n",
    "            for point in new_points :\n",
    "                if checkLimits(point[0],point[1]) == True:\n",
    "\n",
    "                    get_x, get_y = get_grid_coordinates(point[0], point[1])\n",
    "                    \n",
    "                    check = check_occupancy(point[0], point[1])\n",
    "                   \n",
    "                else :\n",
    "                    check =1\n",
    "\n",
    "                if check == 1:\n",
    "                \n",
    "                   break\n",
    "\n",
    "            if check == 0:\n",
    "                collision_free.append(state_list[i])\n",
    "                \n",
    "\n",
    "        if( collision_free ):\n",
    "\n",
    "            for nstate in collision_free:\n",
    "                \n",
    "                #print( nstate.x, nstate.y, nstate.theta )\n",
    "                new_link = link( nstate, len(trace_list) )\n",
    "                open_list.append( new_link )\n",
    "                \n",
    "                trace_link = link( nstate, path_index )\n",
    "                trace_list.append( trace_link )\n",
    "                          \n",
    "                \n",
    "                if np.sqrt((nstate.x - Goal.x)**2 + (nstate.y - Goal.y)**2) < 0.5:\n",
    "                    goal_reached = 1\n",
    "                    path_x.append( nstate.x )\n",
    "                    path_y.append( nstate.y )\n",
    "                    print( \"goal\")\n",
    "                    \n",
    "\n",
    "\n",
    "            #path_state = collision_free[0]\n",
    "\n",
    "            #if checkLimits( path_state ) == True:\n",
    "            #    path_x.append( path_state.x )\n",
    "            #    path_y.append( path_state.y )\n",
    "\n",
    "                #print( path_state.x, path_state.y )\n",
    "\n",
    "\n",
    "    #print( \"\" )\n",
    "    #print( \"Ostates\")\n",
    "    #for ostate in open_list:\n",
    "\n",
    "       # print( ostate.x, ostate.y, ostate.theta, ostate.f )\n",
    "\n",
    "plt.scatter( path_x, path_y )\n",
    "plt.axis('equal')\n",
    "plt.show()\n",
    "        \n",
    "    \n",
    "#print(current_state.x, current_state.y, current_state.theta)\n",
    "#print(new_points)\n",
    "#grid_v = np.copy(grid)\n",
    "\n",
    "#    if check == 1:\n",
    "#        break\n",
    "\n",
    "#if check == 0:\n",
    "#    print(grid_v)\n",
    "\n",
    "#print(grid_v)\n",
    "\n",
    "#plt.scatter(px,py)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "#plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2499,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAXQAAAD8CAYAAABn919SAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nA0BJEOgAUBIEOgCUBIEOACVBoANASfw/oCIu5AKh298AAAAASUVORK5CYII=\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7f9480120a58>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "if goal_reached == 1 :\n",
    "        fpath_x = []\n",
    "        fpath_y = []\n",
    "        \n",
    "        curr_trace = trace_list[-1]\n",
    "        \n",
    "        while( curr_trace.index != -1 ):\n",
    "            fpath_x.append( curr_trace.state.x )\n",
    "            fpath_y.append( curr_trace.state.y )\n",
    "            \n",
    "            curr_trace = trace_list[ curr_trace.index ]\n",
    "            \n",
    "plt.scatter( fpath_x, fpath_y )\n",
    "plt.axis( 'equal' )\n",
    "plt.show()\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2477,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "719\n"
     ]
    }
   ],
   "source": [
    "print( math.floor( 6.277231820095903 * NUM_THETA_CELLS / np.pi ) )\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2472,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAW4AAAD8CAYAAABXe05zAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nCcPATUSUMAzcREQJw8BNRJQw/x9NKdjLWKL56wAAAABJRU5ErkJggg==\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7f9480017ba8>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "plt.scatter( path_x, path_y )\n",
    "plt.axis('equal')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2461,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAX8AAAD9CAYAAABUS3cAAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nQmCC\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7f9480749358>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "steer = 50 * np.pi / 180\n",
    "cstate = state( 0, 0, np.pi/2, 0, 0, steer )\n",
    "px = [] \n",
    "py = []\n",
    "\n",
    "px.append( cstate.x )\n",
    "py.append( cstate.y )\n",
    "\n",
    "for i in range(150):\n",
    "    cstate = transition( cstate, steer, 0.1, Goal)\n",
    "    px.append( cstate.x )\n",
    "    py.append( cstate.y )\n",
    "    \n",
    "plt.scatter( px, py, c='green' )\n",
    "#plt.show()\n",
    "    \n",
    "    \n",
    "steer = 50 * np.pi / 180\n",
    "cstate = state( 0, 0, np.pi/2, 0, 0, steer )\n",
    "px = [] \n",
    "py = []\n",
    "\n",
    "px.append( cstate.x )\n",
    "py.append( cstate.y )\n",
    "\n",
    "for i in range(150):\n",
    "    cstate = transition_tan( cstate, steer, 0.1, Goal)\n",
    "    px.append( cstate.x )\n",
    "    py.append( cstate.y )\n",
    "    \n",
    "plt.scatter( px, py, c='red' )\n",
    "\n",
    "plt.axis('equal')\n",
    "plt.show()\n",
    "    \n",
    "    \n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2257,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 2257,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "checkLimits(1.12514434592, 0.594684959317)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2004,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 4.81600472 -0.31940363]\n",
      " [ 4.75204541 -1.4175426 ]\n",
      " [ 4.6880861  -2.51568158]\n",
      " [ 2.8193884  -0.20311397]\n",
      " [ 2.75542909 -1.30125294]\n",
      " [ 2.69146978 -2.39939192]\n",
      " [ 0.82277209 -0.08682431]\n",
      " [ 0.75881278 -1.18496329]\n",
      " [ 0.69485346 -2.28310226]]\n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYAAAAD8CAYAAAB+UHOxAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nQJIaZQBIUqMMAElqlAEgSY36Xx6BtUyhVtSBAAAAAElFTkSuQmCC\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7f9480988dd8>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "print( new_points )\n",
    "px = []\n",
    "py = []\n",
    "for i in range(len(new_points)):\n",
    "    px.append( new_points[i][0] )\n",
    "    py.append( new_points[i][1] )\n",
    "\n",
    "plt.scatter(px,py)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2005,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]]\n"
     ]
    }
   ],
   "source": [
    "print(grid)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1790,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 1790,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "check_occupancy(0,10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(10, 10)"
      ]
     },
     "execution_count": 77,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "get_grid_coordinates(0, 10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 803,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]]\n"
     ]
    }
   ],
   "source": [
    "print(grid)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 701,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "NUM_THETA_CELLS = 90"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Takes an angle (in radians) and returns which \"stack\" in the 3D configuration space\n",
    "this angle corresponds to. Angles near 0 go in the lower stacks while angles near \n",
    "2 * pi go in the higher stacks."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 704,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def theta_to_stack_number(theta):\n",
    "  \n",
    "    new_theta = math.fmod((theta + 2 * math.pi),(2 * math.pi))\n",
    "    stack_number = (round(new_theta * NUM_THETA_CELLS / (2*math.pi))) % NUM_THETA_CELLS\n",
    "    return stack_number\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 705,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "14\n"
     ]
    }
   ],
   "source": [
    "print(theta_to_stack_number(1))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 706,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def idx (n):\n",
    "    return math.floor(n)\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 707,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "class start:\n",
    "\n",
    "    def __init__(self, x, y, theta):\n",
    "        self.x = x\n",
    "        self.y = y\n",
    "        self.theta = theta\n",
    "        \n",
    "class goal:\n",
    "\n",
    "    def __init__(self, x, y):\n",
    "        self.x = x\n",
    "        self.y = y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 828,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "True"
      ]
     },
     "execution_count": 828,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "deg_to_rad(90) == np.pi/2\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 862,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.17453292519943295, 0.2617993877991494, 0.3490658503988659, 0.4363323129985824, 0.5235987755982988, 0.6108652381980153, 0.6981317007977318, 0.7853981633974483, 0.8726646259971648, 0.9599310885968813, 1.0471975511965976, 0.08726646259971647, 0.0, -0.08726646259971647, -0.17453292519943295, -0.2617993877991494, -0.3490658503988659, -0.4363323129985824, -0.5235987755982988, -0.6108652381980153, -0.6981317007977318]\n",
      "21\n"
     ]
    }
   ],
   "source": [
    "angle = delta_t(10, 10, 5)\n",
    "print(angle)\n",
    "print(len(angle))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 870,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def search(grid, start, goal):\n",
    "    # The opened array keeps track of the stack of States objects we are \n",
    "    # searching through.\n",
    "    opened = []\n",
    "    # 3D array of zeros with dimensions:\n",
    "    # (NUM_THETA_CELLS, grid x size, grid y size).\n",
    "    closed = [[[0 for x in range(len(grid[0]))] for y in range(len(grid))] for cell in range(NUM_THETA_CELLS)]\n",
    "    # 3D array with same dimensions. Will be filled with State() objects to keep \n",
    "    # track of the path through the grid. \n",
    "    came_from = [[[0 for x in range(len(grid[0]))] for y in range(len(grid))] for cell in range(NUM_THETA_CELLS)]\n",
    "\n",
    "    # Create new state object to start the search with.\n",
    "    x = start.x\n",
    "    y = start.y\n",
    "    \n",
    "    theta = start.theta\n",
    "    g = 0\n",
    "    f = heuristic(x, y, goal)\n",
    "    ste = state(x, y, theta, 0, f)\n",
    "    opened.append(ste)\n",
    "    print(\"Reached\")\n",
    "\n",
    "    # The range from 0 to 2pi has been discretized into NUM_THETA_CELLS cells. \n",
    "    # Here, theta_to_stack_number returns the cell that theta belongs to. \n",
    "    # Smaller thetas (close to 0 when normalized  into the range from 0 to 2pi) \n",
    "    # have lower stack numbers, and larger thetas (close to 2pi whe normalized)\n",
    "    # have larger stack numbers.\n",
    "    stack_number = theta_to_stack_number(ste.theta)\n",
    "    closed[stack_number][idx(ste.x)][idx(ste.y)] = 1\n",
    "    print(\"Reached\")\n",
    "\n",
    "    # Store our starting state. For other states, we will store the previous state \n",
    "    # in the path, but the starting state has no previous.\n",
    "    came_from[stack_number][idx(ste.x)][idx(ste.y)] = ste\n",
    "    print(\"Reached\")\n",
    "\n",
    "    # While there are still states to explore:\n",
    "    while opened:\n",
    "        # Sort the states by f-value and start search using the state with the \n",
    "        # lowest f-value. This is crucial to the A* algorithm; the f-value \n",
    "        # improves search efficiency by indicating where to look first.\n",
    "        opened.sort(key=lambda ste:ste.f)\n",
    "        current = opened.pop(0)\n",
    "        print(current.x , current.y, current.theta)\n",
    "        print(len(opened))\n",
    "\n",
    "        # Check if the x and y coordinates are in the same grid cell as the goal. \n",
    "        # (Note: The idx function returns the grid index for a given coordinate.)\n",
    "        if (idx(current.x) == goal.x) and (idx(current.y) == goal.y):\n",
    "            # If so, the trajectory has reached the goal.\n",
    "            print(path)\n",
    "            print(\"Reached\")\n",
    "            return path\n",
    "\n",
    "        # Otherwise, expand the current state to get a list of possible next states.\n",
    "        next_states = expand(current, goal)\n",
    "        \n",
    "        #print(len(next_states))\n",
    "        for i in range(len(next_states)):\n",
    "            \n",
    "            print(next_states[i].f)\n",
    "        for next_state in next_states:\n",
    "            # If we have expanded outside the grid, skip this next_state.\n",
    "            if next_states not in grid:\n",
    "                continue\n",
    "            # Otherwise, check that we haven't already visited this cell and\n",
    "            # that there is not an obstacle in the grid there.\n",
    "            stack_number = theta_to_stack_number(next_state.theta)\n",
    "            print(\"Reached\")\n",
    "            if closed_value[stack_number][idx(next_state.x)][idx(next_state.y)] == 0 and grid[idx(next_state.x)][idx(next_state.y)] == 0:\n",
    "                # The state can be added to the opened stack.\n",
    "                opened.append(next_state)\n",
    "                # The stack_number, idx(next_state.x), idx(next_state.y) tuple \n",
    "                # has now been visited, so it can be closed.\n",
    "                closed[stack_number][idx(next_state.x)][idx(next_state.y)] = 1\n",
    "                # The next_state came from the current state, and that is recorded.\n",
    "                came_from[stack_number][idx(next_state.x)][idx(next_state.y)] = current"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 871,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reached\n",
      "Reached\n",
      "Reached\n",
      "0 0 0\n",
      "0\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n",
      "31.032648900821254\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/ros/anaconda3/lib/python3.6/site-packages/ipykernel_launcher.py:64: DeprecationWarning: elementwise == comparison failed; this will raise an error in the future.\n"
     ]
    }
   ],
   "source": [
    "Start = start(0, 0, 0)\n",
    "Goal = goal(0, 30)\n",
    "search(grid, Start, Goal)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 768,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]\n",
      " [0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0]]\n"
     ]
    }
   ],
   "source": [
    "print(grid)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 735,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def search(grid, start, goal):\n",
    "    # The opened array keeps track of the stack of States objects we are \n",
    "    # searching through.\n",
    "    opened = []\n",
    "    # 3D array of zeros with dimensions:\n",
    "    # (NUM_THETA_CELLS, grid x size, grid y size).\n",
    "    closed = [[[0 for x in range(len(grid[0]))] for y in range(len(grid))] for cell in range(NUM_THETA_CELLS)]\n",
    "    # 3D array with same dimensions. Will be filled with State() objects to keep \n",
    "    # track of the path through the grid. \n",
    "    came_from = [[[0 for x in range(len(grid[0]))] for y in range(len(grid))] for cell in range(NUM_THETA_CELLS)]\n",
    "\n",
    "    # Create new state object to start the search with.\n",
    "    x = start.x\n",
    "    y = start.y\n",
    "    \n",
    "    theta = start.theta\n",
    "    g = 0\n",
    "    f = heuristic(x, y, goal)\n",
    "    v_state = state(x, y, theta, 0, f)\n",
    "    opened.append(v_state)\n",
    "\n",
    "    # The range from 0 to 2pi has been discretized into NUM_THETA_CELLS cells. \n",
    "    # Here, theta_to_stack_number returns the cell that theta belongs to. \n",
    "    # Smaller thetas (close to 0 when normalized  into the range from 0 to 2pi) \n",
    "    # have lower stack numbers, and larger thetas (close to 2pi whe normalized)\n",
    "    # have larger stack numbers.\n",
    "    stack_number = theta_to_stack_number(v_state.theta)\n",
    "    closed[stack_number][idx(v_state.x)][idx(v_state.y)] = 1\n",
    "\n",
    "    # Store our starting state. For other states, we will store the previous state \n",
    "    # in the path, but the starting state has no previous.\n",
    "    came_from[stack_number][idx(v_state.x)][idx(v_state.y)] = state\n",
    "\n",
    "    # While there are still states to explore:\n",
    "    while opened:\n",
    "        # Sort the states by f-value and start search using the state with the \n",
    "        # lowest f-value. This is crucial to the A* algorithm; the f-value \n",
    "        # improves search efficiency by indicating where to look first.\n",
    "        opened.sort(key=lambda v_state:v_state.f)\n",
    "        current = opened.pop(0)\n",
    "\n",
    "        # Check if the x and y coordinates are in the same grid cell as the goal. \n",
    "        # (Note: The idx function returns the grid index for a given coordinate.)\n",
    "        if (idx(current.x) == goal.x) and (idx(current.y) == goal.y):\n",
    "            # If so, the trajectory has reached the goal.\n",
    "            return path\n",
    "\n",
    "        # Otherwise, expand the current state to get a list of possible next states.\n",
    "        next_states = expand(current, goal)\n",
    "        for next_state in next_states:\n",
    "            # If we have expanded outside the grid, skip this next_state.\n",
    "            if next_states not in grid:\n",
    "                continue\n",
    "            # Otherwise, check that we haven't already visited this cell and\n",
    "            # that there is not an obstacle in the grid there.\n",
    "            stack_number = theta_to_stack_number(next_state.theta)\n",
    "            if closed_value[stack_number][idx(next_state.x)][idx(next_state.y)] == 0 and grid[idx(next_state.x)][idx(next_state.y)] == 0:\n",
    "                # The state can be added to the opened stack.\n",
    "                opened.append(next_state)\n",
    "                # The stack_number, idx(next_state.x), idx(next_state.y) tuple \n",
    "                # has now been visited, so it can be closed.\n",
    "                closed[stack_number][idx(next_state.x)][idx(next_state.y)] = 1\n",
    "                # The next_state came from the current state, and that is recorded.\n",
    "                came_from[stack_number][idx(next_state.x)][idx(next_state.y)] = current"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 769,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Reached\n",
      "Reached\n",
      "Reached\n",
      "Reached\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/ros/anaconda3/lib/python3.6/site-packages/ipykernel_launcher.py:58: DeprecationWarning: elementwise == comparison failed; this will raise an error in the future.\n"
     ]
    }
   ],
   "source": [
    "Start = start(0, 10, 0)\n",
    "Goal = goal(5, 30)\n",
    "search(grid, Start, Goal)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 715,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "x = np.array([[[5,6],[7, 8]],[[9, 10],[11, 12]]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
